#include "mpccbf_line.h"

namespace mpccbf {

    ReferenceLine::ReferenceLine(const std:: vector<std::pair<double, double>>& xy_points) {   /*规划完后的路径*/
        xy_points_ = xy_points;
    }

    bool ReferenceLine::ComputePathProfile(std::vector<double>* headings, std::vector<double>* accumulated_s) {   /*计算航向角和累计弧长*/
        headings->clear();
        accumulated_s->clear();
        if (xy_points_.size() < 2) return false;

        std::vector<double> dxs;
        std::vector<double> dys;

        // Get finite difference approximated dx,dy for heading and kappa
        std::size_t points_size = xy_points_.size();
        for (std::size_t i = 0; i < points_size; i++) {
            double x_delta = 0.0;
            double y_delta = 0.0;
            if (i == 0) {
                x_delta = (xy_points_[i + 1].first - xy_points_[i].first);
                y_delta = (xy_points_[i + 1].second - xy_points_[i].second);
            }
            else if (i == points_size - 1) {
                x_delta = (xy_points_[i].first - xy_points_[i - 1].first);
                y_delta = (xy_points_[i].second - xy_points_[i - 1].second);
            }
            else {
                x_delta = 0.5 * (xy_points_[i + 1].first - xy_points_[i - 1].first);
                y_delta = 0.5 * (xy_points_[i + 1].second - xy_points_[i - 1].second);
            }
            dxs.push_back(x_delta);
            dys.push_back(y_delta);
        }

        // Calculate heading
        for (std::size_t i = 0; i < points_size; i++) {
            headings->push_back(std::atan2(dys[i], dxs[i]));
        }         

        // Get linear interpolated s for dkappa
        double distance = 0.0;
        accumulated_s->push_back(distance);
        double fx = xy_points_[0].first;
        double fy = xy_points_[0].second;
        double nx = 0.0;
        double ny = 0.0;
        for (std::size_t i = 1; i < points_size; i++) {
            nx = xy_points_[i].first;
            ny = xy_points_[i].second;
            double end_segmen_s = std::sqrt((fx - nx) * (fx - nx) + (fy - ny) * (fy - ny));
            accumulated_s->push_back(end_segmen_s + distance);
            distance += end_segmen_s;
            fx = nx;
            fy = ny;
        }
        return true;
    }
}